Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/services timestamps #369

Merged
merged 4 commits into from
Apr 24, 2020
Merged

Feature/services timestamps #369

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
86dcd8d
remember the sample info on requests and responses
iluetkeb Apr 16, 2020
3c10cdf
This implements services timestamps
iluetkeb Apr 21, 2020
68f7797
use rmw_time_point_value_t instead of rmw_time_t
iluetkeb Apr 22, 2020
b43a85b
snake_case
iluetkeb Apr 23, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion rmw_fastrtps_cpp/src/rmw_request.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
2 changes: 1 addition & 1 deletion rmw_fastrtps_cpp/src/rmw_response.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
2 changes: 1 addition & 1 deletion rmw_fastrtps_dynamic_cpp/src/rmw_request.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
2 changes: 1 addition & 1 deletion rmw_fastrtps_dynamic_cpp/src/rmw_response.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
8 changes: 4 additions & 4 deletions rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,7 @@ typedef struct CustomClientResponse
{
eprosima::fastrtps::rtps::SampleIdentity sample_identity_;
std::unique_ptr<eprosima::fastcdr::FastBuffer> buffer_;
eprosima::fastrtps::SampleInfo_t sample_info_ {};
} CustomClientResponse;

class ClientListener : public eprosima::fastrtps::SubscriberListener
Expand All @@ -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<std::mutex> lock(internalMutex_);
Expand Down
8 changes: 4 additions & 4 deletions rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {}
Expand All @@ -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<std::mutex> lock(internalMutex_);

Expand Down
4 changes: 2 additions & 2 deletions rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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);

Expand Down
7 changes: 4 additions & 3 deletions rmw_fastrtps_shared_cpp/src/rmw_request.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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) <<
iluetkeb marked this conversation as resolved.
Show resolved Hide resolved
32 | request.sample_identity_.sequence_number().low;

delete request.buffer_;
Expand Down
7 changes: 5 additions & 2 deletions rmw_fastrtps_shared_cpp/src/rmw_response.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
Expand Down