From aad47620a14310a2ccabcb7cd02d528de47ac892 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ingo=20L=C3=BCtkebohle?= Date: Fri, 24 Apr 2020 06:31:20 +0200 Subject: [PATCH] Feature/services timestamps (#369) * remember the sample info on requests and responses Signed-off-by: Luetkebohle Ingo (CR/AEX3) * This implements services timestamps Requires https://github.com/ros2/rmw/pull/217 Realizes the 2nd part of https://github.com/ros2/design/issues/259 Signed-off-by: Luetkebohle Ingo (CR/AEX3) * use rmw_time_point_value_t instead of rmw_time_t Signed-off-by: Luetkebohle Ingo (CR/AEX3) * snake_case Signed-off-by: Luetkebohle Ingo (CR/AEX3) --- rmw_fastrtps_cpp/src/rmw_request.cpp | 2 +- rmw_fastrtps_cpp/src/rmw_response.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp | 2 +- .../rmw_fastrtps_shared_cpp/custom_client_info.hpp | 8 ++++---- .../rmw_fastrtps_shared_cpp/custom_service_info.hpp | 8 ++++---- .../include/rmw_fastrtps_shared_cpp/rmw_common.hpp | 4 ++-- rmw_fastrtps_shared_cpp/src/rmw_request.cpp | 7 ++++--- rmw_fastrtps_shared_cpp/src/rmw_response.cpp | 7 +++++-- 9 files changed, 23 insertions(+), 19 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_request.cpp b/rmw_fastrtps_cpp/src/rmw_request.cpp index 44a6f44b7..7d33d7db3 100644 --- a/rmw_fastrtps_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_cpp/src/rmw_request.cpp @@ -35,7 +35,7 @@ rmw_send_request( rmw_ret_t rmw_take_request( const rmw_service_t * service, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_request, bool * taken) { diff --git a/rmw_fastrtps_cpp/src/rmw_response.cpp b/rmw_fastrtps_cpp/src/rmw_response.cpp index 47c283abc..0fc3e11a1 100644 --- a/rmw_fastrtps_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_cpp/src/rmw_response.cpp @@ -24,7 +24,7 @@ extern "C" rmw_ret_t rmw_take_response( const rmw_client_t * client, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_response, bool * taken) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp index 9b3a12b91..90d13a5b8 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp @@ -35,7 +35,7 @@ rmw_send_request( rmw_ret_t rmw_take_request( const rmw_service_t * service, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_request, bool * taken) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp index 7aeba853e..9a424a589 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp @@ -24,7 +24,7 @@ extern "C" rmw_ret_t rmw_take_response( const rmw_client_t * client, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_response, bool * taken) { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp index 07318a095..3a48b3c06 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -60,6 +60,7 @@ typedef struct CustomClientResponse { eprosima::fastrtps::rtps::SampleIdentity sample_identity_; std::unique_ptr buffer_; + eprosima::fastrtps::SampleInfo_t sample_info_ {}; } CustomClientResponse; class ClientListener : public eprosima::fastrtps::SubscriberListener @@ -78,15 +79,14 @@ class ClientListener : public eprosima::fastrtps::SubscriberListener CustomClientResponse response; // Todo(sloretz) eliminate heap allocation pending eprosima/Fast-CDR#19 response.buffer_.reset(new eprosima::fastcdr::FastBuffer()); - eprosima::fastrtps::SampleInfo_t sinfo; rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; data.data = response.buffer_.get(); data.impl = nullptr; // not used when is_cdr_buffer is true - if (sub->takeNextData(&data, &sinfo)) { - if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { - response.sample_identity_ = sinfo.related_sample_identity; + if (sub->takeNextData(&data, &response.sample_info_)) { + if (eprosima::fastrtps::rtps::ALIVE == response.sample_info_.sampleKind) { + response.sample_identity_ = response.sample_info_.related_sample_identity; if (response.sample_identity_.writer_guid() == info_->writer_guid_) { std::lock_guard lock(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp index 34d37667a..098bb2093 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -52,6 +52,7 @@ typedef struct CustomServiceRequest { eprosima::fastrtps::rtps::SampleIdentity sample_identity_; eprosima::fastcdr::FastBuffer * buffer_; + eprosima::fastrtps::SampleInfo_t sample_info_ {}; CustomServiceRequest() : buffer_(nullptr) {} @@ -75,15 +76,14 @@ class ServiceListener : public eprosima::fastrtps::SubscriberListener CustomServiceRequest request; request.buffer_ = new eprosima::fastcdr::FastBuffer(); - eprosima::fastrtps::SampleInfo_t sinfo; rmw_fastrtps_shared_cpp::SerializedData data; data.is_cdr_buffer = true; data.data = request.buffer_; data.impl = nullptr; // not used when is_cdr_buffer is true - if (sub->takeNextData(&data, &sinfo)) { - if (eprosima::fastrtps::rtps::ALIVE == sinfo.sampleKind) { - request.sample_identity_ = sinfo.sample_identity; + if (sub->takeNextData(&data, &request.sample_info_)) { + if (eprosima::fastrtps::rtps::ALIVE == request.sample_info_.sampleKind) { + request.sample_identity_ = request.sample_info_.sample_identity; std::lock_guard lock(internalMutex_); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index 59b40b7d9..f3a366ef7 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -187,7 +187,7 @@ rmw_ret_t __rmw_take_request( const char * identifier, const rmw_service_t * service, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_request, bool * taken); @@ -196,7 +196,7 @@ rmw_ret_t __rmw_take_response( const char * identifier, const rmw_client_t * client, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_response, bool * taken); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp index 01a30d020..0d16945cd 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_request.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_request.cpp @@ -72,7 +72,7 @@ rmw_ret_t __rmw_take_request( const char * identifier, const rmw_service_t * service, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_request, bool * taken) { @@ -102,8 +102,9 @@ __rmw_take_request( // Get header rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( request.sample_identity_.writer_guid(), - request_header->writer_guid); - request_header->sequence_number = ((int64_t)request.sample_identity_.sequence_number().high) << + request_header->request_id.writer_guid); + request_header->request_id.sequence_number = + ((int64_t)request.sample_identity_.sequence_number().high) << 32 | request.sample_identity_.sequence_number().low; delete request.buffer_; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp index 0fc3fdb9d..c960c6549 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_response.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_response.cpp @@ -33,7 +33,7 @@ rmw_ret_t __rmw_take_response( const char * identifier, const rmw_client_t * client, - rmw_request_id_t * request_header, + rmw_service_info_t * request_header, void * ros_response, bool * taken) { @@ -62,7 +62,10 @@ __rmw_take_response( info->response_type_support_->deserializeROSmessage( deser, ros_response, info->response_type_support_impl_); - request_header->sequence_number = ((int64_t)response.sample_identity_.sequence_number().high) << + request_header->source_timestamp = response.sample_info_.sourceTimestamp.to_ns(); + request_header->received_timestamp = response.sample_info_.receptionTimestamp.to_ns(); + request_header->request_id.sequence_number = + ((int64_t)response.sample_identity_.sequence_number().high) << 32 | response.sample_identity_.sequence_number().low; *taken = true;